/**
 * @file SolverPPPAR.hpp
 * General Solver.
 */

//============================================================================
//
//  Revision
//
//  2019/08/07
//  create a general solver for multi-gpss ppp solution
//  shjzhang
//
//  2020/01/16
//  remove stateMap/covMap, which makes the program complicated
//  shjzhang.
//
//============================================================================


#include "SolverPPPAR.hpp"
#include "SystemTime.hpp"
#include "Ambiguity.hpp"
#include "AmbDatum.hpp"
#include "NEUUtil.hpp"


#define debug 1
#define detail 0

using namespace gpstk::StringUtils;

namespace gpstk
{

      // Index initially assigned to this class
   int SolverPPPAR::classIndex = 9600000;

      // Returns an index identifying this object.
   int SolverPPPAR::getIndex() const
   { return index; }


      // Returns a string identifying this object.
   std::string SolverPPPAR::getClassName() const
   { return "SolverPPPAR"; }

   void SolverPPPAR::Init(void)
   {
       // set up variables and equations for PPP

       // Warning: not consider the GLONASS system until now
       // Warning: only consider LC combinations
       // Warning: see SolverPPPARUC for UC combinations

       //////////////////////////////////////////////////////////////
       //
       // please augument for GAL/BDS system
       // 
       // wavelengthBLC12 = clight/(f1+f2)
       //
       // BLC12  =  clight/(f1+f2)[( B1 + f2/(f1-f2)*Bw]
       //////////////////////////////////////////////////////////////

   }

   void SolverPPPAR::setUpEquations(void)
   {

       ///////////////////////////////////////////////
       //
       // Now, define variables need to be estimated 
       // 
       ///////////////////////////////////////////////
       
          // These variables are, by default, SourceID-indexed
       Variable dx( TypeID::dStaX );
       Variable dy( TypeID::dStaY );
       Variable dz( TypeID::dStaZ );
       dx.setInitialVariance(100*100);
       dy.setInitialVariance(100*100);
       dz.setInitialVariance(100*100);

       Variable dn( TypeID::dStaLat );
       Variable de( TypeID::dStaLon );
       Variable du( TypeID::dStaH );
       dn.setInitialVariance(100*100);
       de.setInitialVariance(100*100);
       du.setInitialVariance(100*100);

       whiteNoiseDXModel.setSigma(100);
       whiteNoiseDYModel.setSigma(100);
       whiteNoiseDZModel.setSigma(100);

       whiteNoiseDNModel.setSigma(100);
       whiteNoiseDEModel.setSigma(100);
       whiteNoiseDUModel.setSigma(100);

       if(kinematic)
       {
           dx.setModel(&whiteNoiseDXModel);
           dy.setModel(&whiteNoiseDYModel);
           dz.setModel(&whiteNoiseDZModel);
           dn.setModel(&whiteNoiseDNModel);
           de.setModel(&whiteNoiseDEModel);
           du.setModel(&whiteNoiseDUModel);
       }
       else
       {
           dx.setModel(&constantDXModel);
           dy.setModel(&constantDXModel);
           dz.setModel(&constantDXModel);
           dn.setModel(&constantDNModel);
           de.setModel(&constantDEModel);
           du.setModel(&constantDUModel);
       }
       
       VariableSet neuVarSet,xyzVarSet;

       xyzVarSet.insert(dx);
       xyzVarSet.insert(dy);
       xyzVarSet.insert(dz);

       neuVarSet.insert(dn);
       neuVarSet.insert(de);
       neuVarSet.insert(du);

       Variable cdt  ( TypeID::dcdtSta, &wnModel);
       Variable tropo( TypeID::wetMap,  &tropoModel );

       // The following variables are, SourceID and SatID-indexed and ambigity-indexed
       Variable BLC12GPS( TypeID::BLC12, &ambiGPSLC12Model, true, true, true);
       Variable BLC15GPS( TypeID::BLC15, &ambiGPSLC15Model, true, true, true);

       Variable BLC15GAL( TypeID::BLC15, &ambiGALLC15Model, true, true, true);
       Variable BLC17GAL( TypeID::BLC17, &ambiGALLC17Model, true, true, true);
       Variable BLC18GAL( TypeID::BLC18, &ambiGALLC18Model, true, true, true);
       Variable BLC16GAL( TypeID::BLC16, &ambiGALLC16Model, true, true, true);

       Variable BLC27BDS( TypeID::BLC27, &ambiBDSLC27Model, true, true, true);
       Variable BLC26BDS( TypeID::BLC26, &ambiBDSLC26Model, true, true, true);

       BLC12GPS.setInitialVariance(39999.0);
       BLC15GPS.setInitialVariance(39999.0);

       BLC15GAL.setInitialVariance(39999.0);
       BLC17GAL.setInitialVariance(39999.0);
       BLC18GAL.setInitialVariance(39999.0);
       BLC16GAL.setInitialVariance(39999.0);

       BLC27BDS.setInitialVariance(39999.0);
       BLC26BDS.setInitialVariance(39999.0);

       // phase-clock for each-type of GNSS
       Variable cdtLC12GPS  ( TypeID::dcdtStaL12GPS, &wnModel);
       Variable cdtLC15GPS  ( TypeID::dcdtStaL15GPS, &wnModel);

       //
       // Define Equation object for code/phase observations, and add variables 
       //

       // GPS PC
       Equation equPC12GPS( TypeID::prefitC12 );
       Equation equPC15GPS( TypeID::prefitC15 );
       // GPS LC
       Equation equLC12GPS( TypeID::prefitL12 );
       Equation equLC15GPS( TypeID::prefitL15 );

       // GAL PC
       Equation equPC15GAL( TypeID::prefitC15, SatelliteSystem::Galileo );
       Equation equPC17GAL( TypeID::prefitC17, SatelliteSystem::Galileo  );
       Equation equPC18GAL( TypeID::prefitC18, SatelliteSystem::Galileo  );
       Equation equPC16GAL( TypeID::prefitC16, SatelliteSystem::Galileo );
       // GAL LC
       Equation equLC15GAL( TypeID::prefitL15, SatelliteSystem::Galileo );
       Equation equLC17GAL( TypeID::prefitL17, SatelliteSystem::Galileo );
       Equation equLC18GAL( TypeID::prefitL18, SatelliteSystem::Galileo );
       Equation equLC16GAL( TypeID::prefitL16, SatelliteSystem::Galileo );

       // BDS PC
       Equation equPC27BDS( TypeID::prefitC27, SatelliteSystem::BDS );
       Equation equPC26BDS( TypeID::prefitC26, SatelliteSystem::BDS );
       // BDS LC                                        
       Equation equLC27BDS( TypeID::prefitL27, SatelliteSystem::BDS );
       Equation equLC26BDS( TypeID::prefitL26, SatelliteSystem::BDS );

       //////////////////////////
       // insert variables now
       //////////////////////////
       if(useNEU) 
       { 
           // GPS PC
           equPC12GPS.addVarSet(neuVarSet); 
           equPC15GPS.addVarSet(neuVarSet); 
           // GPS LC
           equLC12GPS.addVarSet(neuVarSet); 
           equLC15GPS.addVarSet(neuVarSet); 

           // GAL PC
           equPC15GAL.addVarSet(neuVarSet); 
           equPC17GAL.addVarSet(neuVarSet); 
           equPC18GAL.addVarSet(neuVarSet); 
           equPC16GAL.addVarSet(neuVarSet); 
           // GAL LC
           equLC15GAL.addVarSet(neuVarSet); 
           equLC17GAL.addVarSet(neuVarSet); 
           equLC18GAL.addVarSet(neuVarSet); 
           equLC16GAL.addVarSet(neuVarSet); 

           // BDS PC
           equPC27BDS.addVarSet(neuVarSet); 
           equPC26BDS.addVarSet(neuVarSet); 

           // BDS LC
           equLC27BDS.addVarSet(neuVarSet); 
           equLC26BDS.addVarSet(neuVarSet); 
       }
       else       
       {   
           // GPS PC
           equPC12GPS.addVarSet(xyzVarSet); 
           equPC15GPS.addVarSet(xyzVarSet); 
           // GPS LC
           equLC12GPS.addVarSet(xyzVarSet); 
           equLC15GPS.addVarSet(xyzVarSet); 

           // GAL PC
           equPC15GAL.addVarSet(xyzVarSet); 
           equPC17GAL.addVarSet(xyzVarSet); 
           equPC18GAL.addVarSet(xyzVarSet); 
           equPC16GAL.addVarSet(xyzVarSet); 
           // GAL LC
           equLC15GAL.addVarSet(xyzVarSet); 
           equLC17GAL.addVarSet(xyzVarSet); 
           equLC18GAL.addVarSet(xyzVarSet); 
           equLC16GAL.addVarSet(xyzVarSet); 

           // BDS PC
           equPC27BDS.addVarSet(xyzVarSet); 
           equPC26BDS.addVarSet(xyzVarSet); 

           // BDS LC
           equLC27BDS.addVarSet(xyzVarSet); 
           equLC26BDS.addVarSet(xyzVarSet); 
       }

       // GPS Trop
       equPC12GPS.addVariable(tropo);
       equPC15GPS.addVariable(tropo);
       equLC12GPS.addVariable(tropo);
       equLC15GPS.addVariable(tropo);

       // Insert cdt for datum system and isb for other systems
       equPC12GPS.addVariable(cdt, 1.0); 
       equPC15GPS.addVariable(cdt, 1.0);  

       // insert phase-clock for LC combination
       equLC12GPS.addVariable(cdtLC12GPS, 1.0);  
       equLC15GPS.addVariable(cdtLC15GPS, 1.0);  

       // only for LC 
       // GPS
       equLC12GPS.addVariable(BLC12GPS, LC12_WAVELENGTH_GPS);
       equLC15GPS.addVariable(BLC15GPS, LC15_WAVELENGTH_GPS);


       // define the weight for all combinations relative PC12GPS

          // Phase equations should have higher relative weight
       equLC12GPS.setWeight(10000.0);     // 100.0 * 100.0
       equLC15GPS.setWeight(10000.0);     // 100.0 * 100.0

          ///////////
          // SETTING THE RULES: SETUP EQUATION SYSTEM
          ///////////
       
       // YOU MUST clear the equation system firstly!!!!!
       // only current equations is feed to equSystem
       equSystem.clearEquations();
       
          // Add equation descriptions
       if(usingGPS)
       {
           for(TypeIDSet::const_iterator typeIt=gpsCombTypeSet.begin();
                   typeIt!=gpsCombTypeSet.end();
                   typeIt++)
           {
               if( (*typeIt) == TypeID::LC12 )
               {
                   equSystem.addEquation(equPC12GPS);
                   equSystem.addEquation(equLC12GPS);
               }
               else if ( (*typeIt) == TypeID::LC15 )
               {
                   equSystem.addEquation(equPC15GPS);
                   equSystem.addEquation(equLC15GPS);
               }
           }
       }


   } // end of setUpEquations




      /* Returns a reference to a gnssRinex object after solving
       *  the previously defined equation system.
       *
       * @param gData    Data object holding the data.
       */
   void SolverPPPAR::Process(commonHeader& comHeader, gnssRinex& gData)
       noexcept(false)
   {

      try
      {
         preCompute( gData );
         Compute( gData );
         postCompute( gData );
      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );
         GPSTK_THROW(e);
      }

      return;

   }  // End of method 'SolverPPPAR::Prepare()'



      /* Code to be executed before 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverPPPAR::preCompute( gnssRinex& gRin)
       noexcept(false)
   {
      try
      {
         if(debug)
         {
             cout << "begin of SolverPPPAR:" << gRin.header.source << endl;
             std::list<Equation> descripEquList;
             descripEquList = equSystem.getDescripEqus();
             cout << "size:" << descripEquList.size() << endl;
             for(std::list<Equation>::iterator it=descripEquList.begin();
                     it!=descripEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }
         }

            // Prepare the equation system with current data
         equSystem.Prepare(gRin);


            // Get the number of unknowns being processed
         int numUnknowns( equSystem.getTotalNumVariables() );

            // Get the set with unknowns being processed
         currentUnkSet =  equSystem.getVarUnknowns() ;

         if(debug)
         {
             std::list<Equation> currentEquList;
             currentEquList = equSystem.getCurrentEquationsList();
             for(std::list<Equation>::iterator it=currentEquList.begin();
                     it!=currentEquList.end();
                     ++it)
             {
                 (*it).dump(cout);
             }

             cout << "currentUnkSet are as follows:" << endl;
             for(VariableSet::iterator vIt=currentUnkSet.begin();
                     vIt!=currentUnkSet.end();
                     vIt++)
             {
                 cout << (*vIt) << endl;
             }
         }

         // reset filter
         CommonTime epoch( gRin.header.epoch );

            // Feed the filter with the correct state and covariance matrix
         if( firstTime )
         {

            Vector<double> initialState(numUnknowns, 0.0);
            Matrix<double> initialCov( numUnknowns, numUnknowns, 0.0 );

               // Fill the initialCov matrix
            for( auto v: currentUnkSet )
            {
                int index = v.getNowIndex();
                initialCov(index,index) = v.getInitialVariance();
            }

               // Reset Kalman filter state and covariance matrix
            xhat = initialState; 
            P    = initialCov;

               // No longer first time
            firstTime = false;
         }
         else
         {

               // Adapt the size to the current number of unknowns
            Vector<double> currentState(numUnknowns, 0.0);
            Matrix<double> currentCov(numUnknowns, numUnknowns, 0.0);


               // Fill the state vector
            for( auto v: currentUnkSet )
            {
                int now_index = v.getNowIndex();
                int old_index = v.getPreIndex();
                if( old_index != -1)
                {
                    currentState(now_index) = solution( old_index );
                }
            }

               // Fill the covariance matrix, 2019/08/01, shjzhang
               // modify the code, 2020/01/16, shjzhang

               // We need a copy of 'currentUnkSet'
            VariableSet tempSet(currentUnkSet);
            for( auto v: currentUnkSet ) 
            {
                int nowIndex = v.getNowIndex();
                int oldIndex = v.getPreIndex();

                // old Variable
                if( oldIndex != -1 )
                {
                    for( auto v2: tempSet )
                    {
                        int nowIndex2 = v2.getNowIndex();
                        int oldIndex2 = v2.getPreIndex();

                        // old Variable
                        if( oldIndex2 != -1 )
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = covMatrix(oldIndex, oldIndex2);
                        }
                        else // new variable
                        {
                           currentCov(nowIndex, nowIndex2) 
                               = currentCov(nowIndex2, nowIndex) 
                               = 0.0;
                        }
                    }
                }
                else // new variable
                {
                    // only diagnol element is assigned to the new variable
                    currentCov(nowIndex, nowIndex) = v.getInitialVariance();
                }

                // erase var
                tempSet.erase(v);

            }  // End of for( VariableSet::const_iterator itVar1 = currentUnkSet...'

               // Reset Kalman filter to current state and covariance matrix
            xhat = currentState;
            P    = currentCov;

         }  // End of 'if(firstTime)'

            // State Transition Matrix (PhiMatrix)
         phiMatrix = equSystem.getPhiMatrix();
         qMatrix = equSystem.getQMatrix();

/*
         if(debug)
         {
            cout << "phiMatrix" << endl;
            cout <<  phiMatrix  << endl;
            cout << "qMatrix" << endl;
            cout <<  qMatrix  << endl;
         }
*/

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverPPPAR::preCompute()'



      // Compute the solution of the given equations set.
      //
      // @param gData    Data object holding the data.
      //
      // \warning A typical kalman filter will be devided into 
      // two different phase, the prediction and the correction.
      // for some special application, for example, the 'integer'
      // satellite and receiver clock/bias estimation, the ambiguity
      // constraints are generated using the 'predicted' ambiguity 
      // values and its variances. 
      //
   gnssRinex& SolverPPPAR::Compute( gnssRinex& gRin )
       noexcept(false)
   {
      TimeUpdate( phiMatrix, qMatrix );
      Constraint();
      MeasUpdate( measVector, hMatrix, rMatrix );

         // Return  
      return gRin;

   }  // End of method 'SolverPPPAR::Compute()'


      // Predict the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverPPPAR::TimeUpdate( const Matrix<double>& phiMatrix,
                                 const Matrix<double>& processNoiseCovariance )
       noexcept(false)
   {

         // Check whether the phiMatrix is square
      if (!(phiMatrix.isSquare()))
      {
         InvalidSolver e("phiMatrix is not square");
         GPSTK_THROW(e);
      }

         // Get the number of unknowns being processed
      int numUnknowns( equSystem.getTotalNumVariables() );
      int stateSize(xhat.size());
      if(!(stateSize==numUnknowns))
      {
         InvalidSolver e("TimeUpdate(): Size of a posteriori state estimation vector \
do not match the number of unknowns");
         GPSTK_THROW(e);
      }

      int phiRow = static_cast<int>(phiMatrix.rows());
      if (!(phiRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of phiMatrix");
         GPSTK_THROW(e);
      }

      if (!(qMatrix.isSquare()))
      {
         InvalidSolver e("qMatrix is not square");
         GPSTK_THROW(e);
      }

      int qRow = static_cast<int>(qMatrix.rows());
      if (!(qRow==numUnknowns))
      {
         InvalidSolver e("Number of unknowns does not match dimension \
of qMatrix");
         GPSTK_THROW(e);
      }

      try
      {
            // Compute the a priori state vector
         xhatminus = phiMatrix*xhat;
         Matrix<double> phiT(transpose(phiMatrix));
            // Compute the a priori estimate error covariance matrix
         Pminus = phiMatrix*P*phiT + processNoiseCovariance;

      }
      catch(...)
      {
         InvalidSolver e("TimeUpdate(): Unable to predict next state.");
         GPSTK_THROW(e);
         return -1;
      }
        
      return 0;

   }  // End of method 'SolverPPPAR::Compute()'

   void SolverPPPAR::Constraint()
   {
          // Get the number of unknowns being processed
       int numEqu( equSystem.getEquNumber() );
       int numUnknowns( equSystem.getTotalNumVariables() );

          // Get matrices and vectors out of equation system
          // Measurements vector (Prefit-residuals)
       measVector = equSystem.getPrefitsVector();
       hMatrix = equSystem.getGeometryMatrix();
       rMatrix = equSystem.getWeightsMatrix();

       // now, constraint equation
       int numEquALL(numEqu+3);

       Vector<double> tempPrefit(numEquALL, 0.0);
       Matrix<double> tempGeometry(numEquALL, numUnknowns, 0.0);
       Matrix<double> tempWeight(numEquALL, numEquALL, 0.0);

       for(int i=0; i<numEqu; i++)
       {
             // Prefit
          tempPrefit(i) = measVector(i);
             // Geometry
          for(int j=0; j<numUnknowns; j++)
          {
             tempGeometry(i,j) = hMatrix(i,j);
          }
             // Weight
          tempWeight(i,i) = rMatrix(i,i);
       }

       // now, N/E/U constraint
       if(useNEU)
       {
           Variable varDN = getVariable(TypeID::dStaLat, currentUnkSet);
           Variable varDE = getVariable(TypeID::dStaLon, currentUnkSet);
           Variable varDU = getVariable(TypeID::dStaH,   currentUnkSet);

           int idxDN = varDN.getNowIndex();
           int idxDE = varDE.getNowIndex();
           int idxDU = varDU.getNowIndex();
           
           // dN
           tempPrefit(numEqu)             = 0;
           tempPrefit(numEqu+1)           = 0;
           tempPrefit(numEqu+2)           = 0;

           tempGeometry(numEqu,   idxDN)  = 1.0;
           tempGeometry(numEqu+1, idxDE)  = 1.0;
           tempGeometry(numEqu+2, idxDU)  = 1.0;

           tempWeight(numEqu,   numEqu)   = 1.0/(sigLat*sigLat) ;
           tempWeight(numEqu+1, numEqu+1) = 1.0/(sigLon*sigLon) ;
           tempWeight(numEqu+2, numEqu+2) = 1.0/(sigH*sigH) ;

       }
       // now, x/y/z constraint
       else
       {
           Variable dStaX = getVariable(TypeID::dStaX, currentUnkSet);
           Variable dStaY = getVariable(TypeID::dStaY, currentUnkSet);
           Variable dStaZ = getVariable(TypeID::dStaZ, currentUnkSet);

           int idxDX = dStaX.getNowIndex();
           int idxDY = dStaY.getNowIndex();
           int idxDZ = dStaZ.getNowIndex();
           
           tempPrefit(numEqu)             = 0;
           tempPrefit(numEqu+1)           = 0;
           tempPrefit(numEqu+2)           = 0;

           tempGeometry(numEqu,   idxDX)  = 1.0;
           tempGeometry(numEqu+1, idxDY)  = 1.0;
           tempGeometry(numEqu+2, idxDZ)  = 1.0;

           tempWeight(numEqu,   numEqu)   = 1.0/(sigX*sigX) ;
           tempWeight(numEqu+1, numEqu+1) = 1.0/(sigY*sigY) ;
           tempWeight(numEqu+2, numEqu+2) = 1.0/(sigZ*sigZ) ;
       }

       measVector = tempPrefit;
       hMatrix = tempGeometry;
       rMatrix = tempWeight;
/*
       if(debug)
       {
           cout << "measVector" << endl;
           cout << measVector << endl;
           cout << hMatrix<< endl;
           cout << rMatrix<< endl;
       }
*/

       //////////////////////////////////////
       //
       // Ambiguity constraint 
       //
       //////////////////////////////////////

       // corrent corrected BLC;
       VariableDataMap NLCData;
       VariableDataMap NLCVarData;

       // firstly, extract ambiguties from SolverMWAR;
       VariableDataMap currentNwData;

       if(forwardProcessing)
       {
           VariableDataMap allNwData;
           // GPSNw12
           VariableDataMap gpsNw12Data;
           if(pGPSNw12Filter!=NULL)
           {
               gpsNw12Data = pGPSNw12Filter->getCurrentFixedAmbData();
           }
           allNwData.insert(gpsNw12Data.begin(), gpsNw12Data.end());

           // GPSNw15
           VariableDataMap gpsNw15Data;
           if(pGPSNw15Filter!=NULL)
           {
               gpsNw15Data = pGPSNw15Filter->getCurrentFixedAmbData();
           }
           allNwData.insert(gpsNw15Data.begin(), gpsNw15Data.end());

           for(auto v: currentUnkSet)
           {
               if(allNwData.find(v)!=allNwData.end())
               {
                   currentNwData[v] = allNwData[v];
               }
           }
       }
       else
       {
           //////////////////
           //                
           // Warning:
           //
           // please test the code, the index of the Nw is different
           // with that in currentUnkSet;
           //                
           //////////////////
           VariableDataMap allNwData;

           VariableDataMap gpsNw12Data;
           if(pGPSNw12Filter!=NULL)
           {
               gpsNw12Data = pGPSNw12Filter->getAllFixedAmbData();
           }
           allNwData.insert(gpsNw12Data.begin(), gpsNw12Data.end());

           VariableDataMap gpsNw15Data;
           if(pGPSNw15Filter!=NULL)
           {
               gpsNw15Data = pGPSNw15Filter->getAllFixedAmbData();
           }
           allNwData.insert(gpsNw15Data.begin(), gpsNw15Data.end());

           for(auto v: currentUnkSet)
           {
               if(allNwData.find(v)!=allNwData.end())
               {
                   currentNwData[v] = allNwData[v];
               }
           }

       }

       ///////////////////
       //
       // BLC = B1 + f1/(f1-f2)*Nw
       // NLC = B1 = BLC - f1/(f1-f2)*Nw
       //
       ///////////////////
       if(debug)
       {
           cout << "currentNwData" << endl;
           for(auto nd: currentNwData)
           {
               cout << nd.first << ":" << nd.second << endl;
           }
       }
       correctBLC(currentNwData, xhatminus, Pminus, NLCData, NLCVarData);

       if(debug)
       {
           cout << "NLCData" << endl;
           for(auto nd: NLCData)
           {
               cout << nd.first << ":" << nd.second << "var:" << NLCVarData[nd.first] << endl;
           }
       }

       // define ambiguity vars
       VariableDataMap datumNLCData;
       SatelliteSystem::Systems satSys;                        
       TypeID ambType;                 

       // now, define different datum for different satSys and ambType
       VariableDataMap gpsNLC12Data;
       VariableDataMap gpsNLC12VarData;

       bool hasGPSBLC12(false);
       satSys = SatelliteSystem::GPS;
       ambType = TypeID::BLC12;

       // extract GPS-BLC12 data from NLCData;
       hasGPSBLC12 = extractAmbData(satSys, ambType,
                                    NLCData, NLCVarData,
                                    gpsNLC12Data, gpsNLC12VarData);

       if(debug)
       {
           cout << "gpsNLC12Data" << endl;
           for(auto nd: gpsNLC12Data)
           {
               cout << nd.first << nd.second << " var:" << gpsNLC12VarData[nd.first] << endl;
           }
       }

       if(hasGPSBLC12)
       {
           AmbDatum ambDatum;
           VariableDataMap datumGPSNLC12 = ambDatum.Prepare(gpsNLC12Data,gpsNLC12VarData);
           datumNLCData.insert( datumGPSNLC12.begin(), datumGPSNLC12.end() );
       }

       if(debug)
       {
           cout << "datumNLCData" << endl;
           for(auto nd: datumNLCData)
           {
               cout << nd.first << nd.second << endl;
           }
       }

       // now, define different datum for different satSys and ambType
       VariableDataMap gpsNLC15Data;
       VariableDataMap gpsNLC15VarData;

       bool hasGPSBLC15(false);
       satSys = SatelliteSystem::GPS;
       ambType = TypeID::BLC15;

       // extract GPS-BLC15 data from NLCData;
       hasGPSBLC15 = extractAmbData(satSys, ambType,
                                    NLCData, NLCVarData,  
                                    gpsNLC15Data, gpsNLC15VarData);

       if(hasGPSBLC15)
       {
           AmbDatum ambDatum;
           VariableDataMap datumGPSNLC15 = ambDatum.Prepare(gpsNLC15Data,gpsNLC15VarData);
           datumNLCData.insert( datumGPSNLC15.begin(), datumGPSNLC15.end() );
       }


       // now, define different datum for different satSys and ambType
       VariableDataMap galNLC15Data;
       VariableDataMap galNLC15VarData;

       // Galiloe BLC15 
       bool hasGALBLC15(false);

       satSys = SatelliteSystem::Galileo;
       ambType = TypeID::BLC15;

       // extract Galileo-BLC15 data from NLCData;
       hasGALBLC15 = extractAmbData(satSys, ambType,
                                    NLCData, NLCVarData,
                                    galNLC15Data, galNLC15VarData);

       if(hasGALBLC15)
       {
           AmbDatum ambDatum;
           VariableDataMap datumGALNLC15 = ambDatum.Prepare(galNLC15Data,galNLC15VarData);
           datumNLCData.insert( datumGALNLC15.begin(), datumGALNLC15.end() );
       }

       if(debug)
       {
           cout << "datumNLCData" << endl;
           for(auto nd: datumNLCData)
           {
               cout << nd.first << ":" << nd.second << endl;
           }
       }

       // now, let's convert datumNLC to datumBLC;
       VariableDataMap datumBLCData;
       convertNLC(currentNwData, datumNLCData, datumBLCData);

       // now, apply ambiguity constraint
       numEqu = measVector.size();
       numUnknowns = currentUnkSet.size();

       numEquALL = ( numEqu + datumBLCData.size() ) ;
       tempPrefit.resize(numEquALL, 0.0);
       tempGeometry.resize(numEquALL, numUnknowns, 0.0);
       tempWeight.resize(numEquALL, numEquALL, 0.0);

       for(int i=0; i<numEqu; i++)
       {
             // Prefit
          tempPrefit(i) = measVector(i);
             // Geometry
          for(int j=0; j<numUnknowns; j++)
          {
             tempGeometry(i,j) = hMatrix(i,j);
          }
             // Weight
          tempWeight(i,i) = rMatrix(i,i);
       }

       int numRow = numEqu;
       for(auto vd: datumBLCData)
       {
           int numCol = vd.first.getNowIndex();
           tempPrefit(numRow) = vd.second;
           tempGeometry(numRow, numCol) = 1.0;
           tempWeight(numRow, numRow) = 1.0E+9;
           numRow++;
       }

       measVector.resize(numEquALL, 0.0);
       hMatrix.resize(numEquALL, numUnknowns, 0.0);
       rMatrix.resize(numEquALL, numEquALL, 0.0);

       measVector = tempPrefit;
       hMatrix = tempGeometry;
       rMatrix = tempWeight;

       if(detail)
       {
           cout << "measVector" << endl;
           cout <<  measVector  << endl;

           cout << "hMatrix" << endl;
           cout <<  hMatrix << endl;

           cout << "rMatrix" << endl;
           cout <<  rMatrix << endl;
       }
   }


      // Correct the state vector and covariance matrix
      //
      // @param gData    Data object holding the data.
      //
   int SolverPPPAR::MeasUpdate( const Vector<double>& prefitResiduals,
                                 const Matrix<double>& designMatrix,
                                 const Matrix<double>& weightMatrix )
       noexcept(false)
   {

         // By default, results are invalid
      valid = false;
      if (!(weightMatrix.isSquare()))
      {
         InvalidSolver e("MeasUpdate(): Weight matrix is not square");
         GPSTK_THROW(e);
      }


      int wRow = static_cast<int>(weightMatrix.rows());
      int pRow = static_cast<int>(prefitResiduals.size());
      if (!(wRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match dimension of \
weightMatrix");
         GPSTK_THROW(e);
      }


      int gRow = static_cast<int>(designMatrix.rows());
      if (!(gRow==pRow))
      {
         InvalidSolver e("MeasUpdate(): prefitResiduals size does not match \
dimension of designMatrix");
         GPSTK_THROW(e);
      }


      int gCol = static_cast<int>(designMatrix.cols());
      int aprioriStateRow = static_cast<int>(xhatminus.size());
      if (!(gCol==aprioriStateRow))
      {
         InvalidSolver e("MeasUpdate(): designMatrix size and a priori state \
estimation vector do not match.");
         GPSTK_THROW(e);
      }


      int pMCol = static_cast<int>(Pminus.cols());
      if ( pMCol != aprioriStateRow )
      {
         InvalidSolver e("MeasUpdate(): Sizes of a priori error covariance \
matrix and a priori state estimation vector do not match.");
         GPSTK_THROW(e);
      }


         // After checking sizes, let's do the real correction work
      Matrix<double> invPMinus;
      Matrix<double> designMatrixT( transpose(designMatrix) );

      try
      {
         invPMinus = inverseChol(Pminus);
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute invPMinus matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
         Matrix<double> invTemp( designMatrixT * weightMatrix * designMatrix +
                                 invPMinus );
            // Compute the a posteriori error covariance matrix
         P = inverseChol( invTemp );
      }
      catch(...)
      {
         InvalidSolver e("MeasUpdate(): Unable to compute P matrix.");
         GPSTK_THROW(e);
         return -1;
      }

      try
      {
            // Compute the a posteriori state estimation
         xhat = P * ( (designMatrixT * weightMatrix * prefitResiduals) + 
                      (invPMinus * xhatminus) );
      }
      catch(Exception e)
      {
         InvalidSolver eis("MeasUpdate(): Unable to compute xhat.");
         GPSTK_THROW(eis);
         return -1;
      }

      xhatminus = xhat;
      Pminus = P;

      solution = xhat;
      covMatrix = P;
         // Compute the postfit residuals Vector
      postfitResiduals = prefitResiduals - (designMatrix * solution);
         // If everything is fine so far, then the results should be valid
      valid = true;

      return 0;

   }  // End of method 'SolverPPPAR::MeasUpdate()'



      /* Code to be executed after 'Compute()' method.
       *
       * @param gData    Data object holding the data.
       */
   gnssRinex& SolverPPPAR::postCompute( gnssRinex& gRin )
       noexcept(false)
   {
      try
      {
         if(debug)
         {
             cout << YDSTime(gRin.header.epoch) << endl;
             cout << "solution:" << endl;
             cout << solution << endl;
         }

         // ambiguity-related parameter

         CommonTime epoch(gRin.header.epoch);

            // save forward-processing data
         if(forwardProcessing)
         {
             // Save needed gnss rinex data for backward-processing
             // Insert forward gnssRinex to forwardRinexData
             gnssRinex gBack(gRin);

             TypeIDSet typeSet;
             if(useNEU)
             {
                typeSet.insert(TypeID::dStaLat);
                typeSet.insert(TypeID::dStaLon);
                typeSet.insert(TypeID::dStaH);
             }
             else
             {
                typeSet.insert(TypeID::dStaX);
                typeSet.insert(TypeID::dStaY);
                typeSet.insert(TypeID::dStaZ);
             }
             typeSet.insert(TypeID::wetMap);
             typeSet.insert(TypeID::weight);
             typeSet.insert(TypeID::satArc);

             typeSet.insert(TypeID::prefitC12);
             typeSet.insert(TypeID::prefitL12);
             typeSet.insert(TypeID::prefitC15);
             typeSet.insert(TypeID::prefitL15);

             // to be extended

             // keep needed observation data for backward processing
             gBack.keepOnlyTypeID(typeSet);

             // let's store the nominal initial position from BasicModel
             nominalPosData[epoch] = gRin.header.srcPos;

             VariableDataMap currentNwData;
             // GPSNw12
             VariableDataMap gpsNw12Data;

             if(pGPSNw12Filter!=NULL)
             {
                 gpsNw12Data = pGPSNw12Filter->getCurrentFixedAmbData();
             }
             currentNwData.insert(gpsNw12Data.begin(), gpsNw12Data.end());

             if(debug)
             {
                 cout << "fixed BWL:" << currentNwData.size() << endl;
                 for(auto nd: currentNwData)
                 {
                     cout << nd.first << ": " << nd.second << endl;
                 }
             }

             // GPSNw15
             VariableDataMap gpsNw15Data;
             if(pGPSNw15Filter!=NULL)
             {
                 gpsNw15Data = pGPSNw15Filter->getCurrentFixedAmbData();
             }
             currentNwData.insert(gpsNw15Data.begin(), gpsNw15Data.end());

             ////////////////////////////////////////
             //
             // correct BLC with Nw
             // BLC = B1 + f1/(f1-f2)*Nw
             // NLC = B1 = BLC - f1/(f1-f2)*Nw
             //
             ////////////////////////////////////////
             VariableDataMap NLCData;
             CovarianceDataMap NLCCovData;
             correctBLC(currentNwData, solution, covMatrix, NLCData, NLCCovData);

             if(debug)
             {
                 cout << "postCompute:NLCData" << endl;
                 for(auto nd: NLCData)
                 {
                     cout << nd.first << ":" << nd.second << endl;
                 }
             }

             // fix ambiguity using round method
             VariableDataMap NLCFixedData;
             partialAR.Round(NLCData, NLCCovData, NLCFixedData );

             if(debug)
             {
                 cout << "round fixed:" <<endl;
                 for(auto nd: NLCFixedData)
                 {
                     cout << nd.first << nd.second << endl;
                 }
             }

             VariableDataMap BLCFixedData;
             convertNLC(currentNwData, NLCFixedData, BLCFixedData);

             // update sols
             Vector<double> stateFixedRound;
             Matrix<double> covFixedRound;
             updateSols(BLCFixedData, solution, covMatrix, stateFixedRound, covFixedRound);

  
             // fix ambiguity using lambda method
             VariableDataMap NLCFixedDataPar;
             double ratio = partialAR.PAR(NLCData, NLCCovData, NLCFixedDataPar);

             if(debug)
             {
                 cout << "par fixed:" <<endl;
                 for(auto nd: NLCFixedDataPar)
                 {
                     cout << nd.first << nd.second << endl;
                 }
             }

             VariableDataMap BLCFixedDataPar;
             convertNLC(currentNwData, NLCFixedDataPar, BLCFixedDataPar);

             // update sols
             Vector<double> stateFixedPar;
             Matrix<double> covFixedPar;
             updateSols(BLCFixedDataPar, solution, covMatrix, stateFixedPar, covFixedPar);


             int numAmb(0);
             for(auto v : currentUnkSet)
             {
                 if( v.getArcIndexed() )
                 {
                     numAmb++;
                 }
             }

             int numSat = gRin.numSats();

             int numFixedRound = BLCFixedData.size();
             int numFixedPar = BLCFixedDataPar.size();

             if(pFixedSolStreamFwd!=NULL)
             {
                 if(ARMethod==1)
                 {
                    printSols(pFixedSolStreamFwd, epoch, 
                              numSat, solution, covMatrix, 
                              stateFixedPar, covFixedPar, numFixedPar, ratio);
                 }
                 else
                 {
                    printSols(pFixedSolStreamFwd, epoch, 
                              numAmb, solution, covMatrix, 
                              stateFixedRound, covFixedRound, numFixedRound);
                 }
             }

             // store the forward rinex data
             forwardRinexData[epoch] = gBack;
             forwardStateData[epoch] = solution;
             forwardCovData[epoch] = covMatrix;
         }

      }
      catch(Exception& u)
      {
            // Throw an exception if something unexpected happens
         ProcessingException e( getClassName() + ":"
                                + StringUtils::asString( getIndex() ) + ":"
                                + u.what() );

         GPSTK_THROW(e);

      }

      return gRin;

   }  // End of method 'SolverPPPAR::postCompute()'

   void SolverPPPAR::PostProcess()
       noexcept(false)
   {
       try
       {
           // now, backward processing
           forwardProcessing=false;

           //////////////////
           //                
           // Warning:
           // please test the code, the index of the Nw is different
           // with that in currentUnkSet;
           //////////////////

           VariableDataMap allNwData;

           VariableDataMap gpsNw12Data;
           if(pGPSNw12Filter!=NULL)
           {
               gpsNw12Data = pGPSNw12Filter->getAllFixedAmbData();
           }
           allNwData.insert(gpsNw12Data.begin(), gpsNw12Data.end());

           VariableDataMap gpsNw15Data;
           if(pGPSNw15Filter!=NULL)
           {
               gpsNw15Data = pGPSNw15Filter->getAllFixedAmbData();
           }
           allNwData.insert(gpsNw15Data.begin(), gpsNw15Data.end());

           // loop all saved data in forward processing
           for(epochGnssRinexMap::reverse_iterator reverseIt = forwardRinexData.rbegin();
               reverseIt != forwardRinexData.rend();
               ++reverseIt)
           {
               CommonTime epoch( (*reverseIt).first );

               gnssRinex gRin( (*reverseIt).second );

               // Backward filtering
               Process(gRin);

               // Get forward state/cov from forwardStateData and forwardCovData
               Vector<double> lastStateVec;
               Matrix<double> lastCovMatrix;

               lastStateVec = forwardStateData[epoch];
               lastCovMatrix = forwardCovData[epoch];

               Vector<double> smoothedStateVec;
               Matrix<double> smoothedCovMatrix;

               // smoothe the forward and backward state and covariance
               smoothSolution(lastStateVec, lastCovMatrix, 
                              smoothedStateVec, smoothedCovMatrix);

               ////////////////////////////////////////
               //
               // correct BLC with Nw
               // BLC = B1 + f1/(f1-f2)*Nw
               // NLC = B1 = BLC - f1/(f1-f2)*Nw
               //
               ////////////////////////////////////////
               VariableDataMap NLCData;
               CovarianceDataMap NLCCovData;
               correctBLC(allNwData, smoothedStateVec, smoothedCovMatrix, 
                          NLCData, NLCCovData);

               if(debug)
               {
                   cout << "NLCData" << endl;
                   for(auto nd: NLCData)
                   {
                       cout << nd.first << ":" << nd.second << endl;
                   }

                   for(auto nd: NLCCovData)
                   {
                       for(auto cd: nd.second)
                       {
                           cout << nd.first << " : " << cd.first << ":" << cd.second << endl;
                       }
                   }
               }

               // now, printout
               if(pBLCStreamSmoothed!=NULL) 
                   printAmbs(pBLCStreamSmoothed, epoch, NLCData, NLCCovData);

               if(debug) 
                   cout << "after printAmbs" << endl;

               ////////////////////
               //
               // Round method
               //
               ////////////////////

               // fix ambiguity using round method
               VariableDataMap NLCFixedData;
               partialAR.Round(NLCData, NLCCovData, NLCFixedData );

               if(debug)
               {
                   cout << "NLCFixedData" << endl;
                   for(auto nd: NLCFixedData)
                   {
                       cout << nd.first << ":" << nd.second << endl;
                   }
               }

               VariableDataMap BLCFixedData;
               convertNLC(allNwData, NLCFixedData, BLCFixedData);

               // update sols
               Vector<double> stateFixedRound;
               Matrix<double> covFixedRound;
               updateSols(BLCFixedData, smoothedStateVec, smoothedCovMatrix,
                          stateFixedRound, covFixedRound);

               ////////////////////
               //
               // Lambda method
               //
               ////////////////////

               // fix ambiguity using lambda method
               VariableDataMap NLCFixedPar;
               double ratio = partialAR.PAR(NLCData, NLCCovData, NLCFixedPar);

               VariableDataMap BLCFixedDataPar;
               convertNLC(allNwData, NLCFixedPar, BLCFixedDataPar);

               if(debug)
               {
                   cout << "NLCFixedPar" << endl;
                   for(auto nd: NLCFixedPar)
                   {
                       cout << nd.first << ":" << nd.second << endl;
                   }
               }

               // update sols
               Vector<double> stateFixedPar;
               Matrix<double> covFixedPar;
               updateSols(BLCFixedData, smoothedStateVec, smoothedCovMatrix,
                          stateFixedPar, covFixedPar);
  
               int numAmb(0);
               for(auto v : currentUnkSet)
               {
                   if( v.getArcIndexed() )
                   {
                       numAmb++;
                   }
               }

               int numSat = gRin.numSats();
               int numFixedRound = BLCFixedData.size();
               int numFixedPar = BLCFixedDataPar.size();


               // print sols
               if(pFixedSolStreamSmoothed!=NULL)
               {
                   if(ARMethod==1)
                   {
                        printSols(pFixedSolStreamSmoothed, epoch, numSat,
                                  smoothedStateVec, smoothedCovMatrix, 
                                  stateFixedPar, covFixedPar, numFixedPar, ratio );
                   }
                   else
                   {
                        printSols(pFixedSolStreamSmoothed, epoch, numSat,
                                  smoothedStateVec, smoothedCovMatrix, 
                                  stateFixedRound, covFixedRound, numFixedRound );
                   }
               }

           } // end of 'for(epoch)'

           // at last free the random
           forwardRinexData.clear();
           forwardStateData.clear();
           forwardCovData.clear();

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );

          GPSTK_THROW(e);

       }

       return;
   }

   void SolverPPPAR::smoothSolution( Vector<double>& oldStateVec, 
                                   Matrix<double>& oldCovMatrix,
                                   Vector<double>& smoothedStateVec, 
                                   Matrix<double>& smoothedCovMatrix)
   {

       try
       {
           if(solution.size() != oldStateVec.size())
           {
              InvalidSolver e("smoothSolution(): size of stateMap don't match size of the oldState");
              GPSTK_THROW(e);
           }

           Matrix<double> currentWeight;
           Matrix<double> oldWeight;

           try
           {
               currentWeight = inverseChol(covMatrix);
               oldWeight = inverseChol(oldCovMatrix);
           }
           catch(Exception& u)
           {
               cerr << "SolverPPPAR::smoothSolution: covMatrix inversion error!" << endl;
               exit(-1);
           }

           smoothedCovMatrix = inverseChol( currentWeight +  oldWeight);
           smoothedStateVec = smoothedCovMatrix * ( currentWeight * solution+ oldWeight *oldStateVec);

       }
       catch(Exception& u)
       {
             // Throw an exception if something unexpected happens
          ProcessingException e( getClassName() + ":"
                                 + StringUtils::asString( getIndex() ) + ":"
                                 + u.what() );
          GPSTK_THROW(e);
       }

   };

   bool SolverPPPAR::extractAmbData(SatelliteSystem::Systems sys, 
                                    TypeID type,
                                    VariableDataMap& NLCData, 
                                    VariableDataMap& NLCVarData,
                                    VariableDataMap& ambData,
                                    VariableDataMap& ambVarData)
   {
       VariableSet ambSet;
       for(auto nd: NLCData)
       {
          ambSet.insert(nd.first);
       }

       bool hasAmb(false);
       if(ambSet.size() > 0)
       {
           hasAmb = true;
       }

       for(auto amb: ambSet)
       {
           ambData[amb] = NLCData[amb];
           ambVarData[amb] = NLCVarData[amb];
       }

       return hasAmb;
   }


   int SolverPPPAR::correctBLC( VariableDataMap& NwData,
                                Vector<double>& solution, 
                                Matrix<double>& covMatrix,
                                VariableDataMap& NLCData,
                                VariableDataMap& NLCVarData)
   {
       CovarianceDataMap NLCCovData;
       correctBLC(NwData, solution, covMatrix, NLCData, NLCCovData);

       for(auto v: NLCCovData)
       {
           NLCVarData[v.first] = NLCCovData[v.first][v.first]; 
       }
   };


   int SolverPPPAR::correctBLC( VariableDataMap& NwData,
                                Vector<double>& solution, 
                                Matrix<double>& covMatrix,
                                VariableDataMap& NLCData,
                                CovarianceDataMap& NLCCovData)
   {


       // according to currentUnkSet, 
       // now, extract ambiguity-related var's state/cov for ambiguty input/outupt
       VariableSet ambVarSet;

       for(auto v : currentUnkSet )
       {
           if(NwData.find(v)!=NwData.end())
           {
               ambVarSet.insert( v );
           }
       }


       for(auto v: ambVarSet )
       {
           SatelliteSystem::Systems satSys = v.getSatSys();
           TypeID ambType = v.getType();

           double coeffNw;
           if( satSys == SatelliteSystem::GPS && 
               ambType == TypeID::BLC12)
           {
                double f1  = L1_FREQ_GPS;
                double f2  = L2_FREQ_GPS;
                coeffNw = f2/(f1-f2);
           }
           else if( satSys == SatelliteSystem::GPS && 
                    ambType == TypeID::BLC15)
           {
                double f1  = L1_FREQ_GPS;
                double f5  = L5_FREQ_GPS;
                coeffNw = f5/(f1-f5);
           }

           double BLC = solution(v.getNowIndex());
           double Nw  = NwData[v];


           ////////////////////
           //  BLC = B1  + f2/(f1-f2)(B1-B2);
           //      = f1/(f1-f2)*B1 - f2/(f1-f2)*B2
           //
           //  for ambiguity resolution:
           //  BLC = B1  + f2/(f1-f2)*Nw 
           //
           //  B1  = BLC - f2/(f1-f2)*Nw;
           //  NLC = B1;
           ////////////////////

           double NLC = BLC - coeffNw*Nw;
           NLCData[v] = NLC;

           for(auto v2: ambVarSet)
           {
               NLCCovData[v][v2] = covMatrix(v.getNowIndex(), v2.getNowIndex());
           }
       }
   };

   int SolverPPPAR::convertNLC( VariableDataMap& NwData,
                                VariableDataMap& NLCData,
                                VariableDataMap& fixedBLCData)
   {

       for(auto nd: NLCData)
       {
           SatelliteSystem::Systems satSys = nd.first.getSatSys();
           TypeID ambType = nd.first.getType();

           double NLC = nd.second;
           double Nw  = NwData[nd.first];

           if(debug)
           cout << "NLC:" << NLC << "Nw:" << Nw << endl; 

           double coeffNw;
           if( satSys == SatelliteSystem::GPS && 
               ambType == TypeID::BLC12)
           {
                double f1  = L1_FREQ_GPS;
                double f2  = L2_FREQ_GPS;
                coeffNw = f2/(f1-f2);
           }
           else if( satSys == SatelliteSystem::GPS && 
                    ambType == TypeID::BLC15)
           {
                double f1  = L1_FREQ_GPS;
                double f5  = L5_FREQ_GPS;
                coeffNw = f5/(f1-f5)*Nw;
           }

           double BLC = NLC + coeffNw*Nw;

           if(debug)
           cout << "BLC:" << BLC << endl; 

           fixedBLCData[nd.first] = BLC;
       }

   };

   int SolverPPPAR::updateSols( VariableDataMap& fixedAmbData, 
                                Vector<double>& state,
                                Matrix<double>& covMatrix,
                                Vector<double>& stateFixed, 
                                Matrix<double>& covMatrixFixed )
   {


       int numAmbs = fixedAmbData.size();
       int numUnks = currentUnkSet.size();

       int numEqu = numAmbs + numUnks;
       Matrix<double> augGeometry(numEqu, numUnks, 0.0);
       Vector<double> augPrefit(numEqu, 0.0);
       Matrix<double> augWeight(numEqu, numEqu, 0.0);

       for(int i=0; i<numUnks; i++)
       {
           augGeometry(i,i) = 1.0;
           augPrefit(i) = state(i);
       }

       // weight
       Matrix<double> wMatrix = inverseChol(covMatrix);
       for(int i=0; i<numUnks; i++)
       {
           for(int j=0; j<numUnks; j++)
           {
               augWeight(i,j) = wMatrix(i,j);
           }
       }

       // geometry/prefit for fixed ambs;
       int i = numUnks;
       for(auto ad: fixedAmbData)
       {
           int j = ad.first.getNowIndex();
           augGeometry(i,j) = 1.0;
           augPrefit(i) = ad.second;
           i++;
       }

       // weight for fixed ambs
       for(int i=numUnks; i<numEqu; i++)
       {
           augWeight(i,i) = 1.0E+9;
       }

       if(detail)
       {
           cout << "updateSols" << endl;
           cout << augGeometry  << endl;
           cout << augPrefit    << endl;
           cout << augWeight    << endl;
       }

       // now, let's update new solution
       Matrix<double> AT;
       AT = transpose(augGeometry);
        
       Matrix<double> tempCovMatrix = AT*augWeight*augGeometry;

       try {
           tempCovMatrix = inverseChol( tempCovMatrix );
       }
       catch(...)
       {
           InvalidSolver e("Unable to invert matrix tempCovMatrix");
           GPSTK_THROW(e);
       }

       Vector<double> tempState= tempCovMatrix * AT * augWeight * augPrefit; 

       stateFixed = tempState;
       covMatrixFixed = tempCovMatrix;

   };

  

   Variable SolverPPPAR::getVariable( const TypeID& type, 
                               const VariableSet& varSet) const
      noexcept(false)
   {

         // Declare an iterator for 'stateMap' and go to the first element
      VariableSet::const_iterator it = varSet.begin();

         // Look for a variable with the same type
      while( (*it).getType() != type &&
             it != varSet.end() )
      {
         ++it;

         // If the same type is not found, throw an exception
         if( it == varSet.end() )
         {
             InvalidRequest e("Type not found in varSet.");
             GPSTK_THROW(e);
         }
      }

         // Else, return the corresponding value
      return (*it);

   }  // End of method 'SolverGeneral::getSolution()'


      /* Returns the solution associated to a given TypeID.
       *
       * @param type    TypeID of the solution we are looking for.
       *
       * \warning In the case the solution contains more than one variable
       * of this type, only the first one will be returned.
       */
   double SolverPPPAR::getSolution( const TypeID& type, 
                                  const Vector<double>& stateVec ) const
      noexcept(false)
   {
         // Declare an varIterator for 'stateMap' and go to the first element
      auto varIt = currentUnkSet.begin();
      while( (*varIt).getType() != type )
      {
         // If the same type is not found, throw an exception
         if( varIt == currentUnkSet.end() )
         {
             InvalidRequest e("Type not found in solution vector.");
             cerr << e << endl;
             exit(-1);
         }

         varIt++;
      }

         // Else, return the corresponding value
      return stateVec( (*varIt).getNowIndex() );

   }  // End of method 'SolverGeneral::getSolution()'



   // print float solutions
   void SolverPPPAR::printSols( std::ofstream* outStr,
                              const CommonTime& epoch, 
                              int numSat, 
                              const Vector<double>& state,
                              const Matrix<double>& covariance,
                              const Vector<double>& stateFixed,
                              const Matrix<double>& covFixed,
                              int numFixed )
   {
       *outStr << YDSTime(epoch).printf("%Y %03j %14.6s") << " ";

       // initial position
       Triple initialPos = nominalPosData[epoch];

       if(debug)
           cout << initialPos << endl;

       if(useNEU )
       {

           Triple neuVec;
           double dLat, dLon, dH, dt;

           // solution1
           dLat = getSolution(TypeID::dStaLat, state);
           dLon = getSolution(TypeID::dStaLon, state);
           dH   = getSolution(TypeID::dStaH,   state);
           dt   = getSolution(TypeID::dcdtSta, state);

           if(detail)
           cout 
               << dLat << " "
               << dLon << " "
               << dH << " "
               << endl;

           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;

           if(detail)
           {
               cout << "neu" << endl;
               cout << neuVec << endl;
           }

           Triple xyzVec;

           // now, let's convert dLat, dLon and dH to
           NEUUtil neuUtil(initialPos);
           xyzVec = neuUtil.convertToXYZ(neuVec);

           if(debug)
           {
               cout << "converted xyz" <<endl;
               cout << xyzVec << endl; 
           }


           Triple estPos = initialPos + xyzVec;

           *outStr << rightJustify(asString(estPos[0],3),   14 )  << " "
                   << rightJustify(asString(estPos[1],3),   14 )  << " "
                   << rightJustify(asString(estPos[2],3),   14 )  << " "
                   << rightJustify(asString(dt  ,3),         9 )  << " "
                   << rightJustify(asString<int>(numSat),4) <<  " "
                   ;

           // solution2
           dLat = getSolution(TypeID::dStaLat, stateFixed);
           dLon = getSolution(TypeID::dStaLon, stateFixed);
           dH   = getSolution(TypeID::dStaH,   stateFixed);
           dt   = getSolution(TypeID::dcdtSta, stateFixed);

           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;

           xyzVec = neuUtil.convertToXYZ(neuVec);
           estPos = initialPos + xyzVec;

           *outStr << rightJustify(asString(estPos[0],3),       14)  << " "
                   << rightJustify(asString(estPos[1],3),       14)  << " "
                   << rightJustify(asString(estPos[2],3),       14)  << " "
                   << rightJustify(asString(dt  ,3),        9)  << " "
                   << rightJustify(asString<int>(numFixed), 4)<< " "
                   ;
           
           *outStr << endl;
       }
       else
       {
           double dx, dy, dz, dt;
           dx = getSolution(TypeID::dStaX,   state);
           dy = getSolution(TypeID::dStaY,   state);
           dz = getSolution(TypeID::dStaZ,   state);
           dt = getSolution(TypeID::dcdtSta, state);

           Triple xyzVec;
           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           Triple estPos = initialPos + xyzVec;

              // print time

           *outStr << rightJustify(asString(estPos[0],6),    14 )<< " "
                   << rightJustify(asString(estPos[1],6),    14 )<< " "
                   << rightJustify(asString(estPos[2],6),    14 )<< " " 
                   << rightJustify(asString(dt,6),           10 )<< " "
                   << rightJustify(asString<int>(numSat),      4) <<  " "
                   ;

           dx = getSolution(TypeID::dStaX,   stateFixed);
           dy = getSolution(TypeID::dStaY,   stateFixed);
           dz = getSolution(TypeID::dStaZ,   stateFixed);
           dt = getSolution(TypeID::dcdtSta, stateFixed);

           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           estPos = initialPos + xyzVec;

           *outStr << rightJustify(asString(estPos[0],6),    14 )<< " "
                   << rightJustify(asString(estPos[1],6),    14 )<< " "
                   << rightJustify(asString(estPos[2],6),    14 )<< " " 
                   << rightJustify(asString(dt,6),           10 )<< " "
                   << rightJustify(asString<int>(numFixed),  10)<< " "
                   ;
           
           *outStr << endl;
       }
   }

   // print float solutions
   void SolverPPPAR::printSols( std::ofstream* outStr,
                                const  CommonTime& epoch, 
                                int    numSat, 
                                const  Vector<double>& state,
                                const  Matrix<double>& covariance,
                                const  Vector<double>& stateFixed,
                                const  Matrix<double>& covFixed,
                                int    numFixed, 
                                double ratio)
   {

       *outStr << YDSTime(epoch).printf("%Y %03j %14.6s") << " ";

       // initial position
       Triple initialPos = nominalPosData[epoch];

       if(debug)
           cout << setprecision(3) << setw(14) << initialPos << endl;

       if(useNEU )
       {

           double dLat, dLon, dH, dt;

           // solution1
           dLat = getSolution(TypeID::dStaLat, state);
           dLon = getSolution(TypeID::dStaLon, state);
           dH   = getSolution(TypeID::dStaH,   state);
           dt   = getSolution(TypeID::dcdtSta, state);

           if(debug)
           cout 
               << dLat << " "
               << dLon << " "
               << dH << " "
               << endl;

           Triple neuVec;
           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;

           Triple xyzVec;

           if(debug)
           {
               cout << "neu" << endl;
               cout << neuVec << endl;
           }

           // now, let's convert dLat, dLon and dH to
           NEUUtil neuUtil(initialPos);
           xyzVec = neuUtil.convertToXYZ(neuVec);

           if(debug)
           {
               cout << "xyzVec" << endl;
               cout << xyzVec << endl;
           }

           Triple estPos = initialPos + xyzVec;

           if(debug)
               cout << "estPos:" << estPos << endl;

           *outStr << rightJustify(asString(estPos[0],3),   12 )  << " "
                   << rightJustify(asString(estPos[1],3),   12 )  << " "
                   << rightJustify(asString(estPos[2],3),   12 )  << " "
                   << rightJustify(asString(dt  ,3),        9  )  << " "
                   << rightJustify(asString<int>(numSat),   4  ) <<  " " 
                   ;

           *outStr << rightJustify(asString(dLat,3), 6 )  << " "
                   << rightJustify(asString(dLon,3), 6 )  << " "
                   << rightJustify(asString(dH  ,3), 6 )  << " "
                   ;

           // solution2
           dLat = getSolution(TypeID::dStaLat, stateFixed);
           dLon = getSolution(TypeID::dStaLon, stateFixed);
           dH   = getSolution(TypeID::dStaH,   stateFixed);
           dt   = getSolution(TypeID::dcdtSta,   stateFixed);

           neuVec[0] = dLat;
           neuVec[1] = dLon;
           neuVec[2] = dH;
           xyzVec = neuUtil.convertToXYZ(neuVec);
           estPos = initialPos + xyzVec;

           *outStr << rightJustify(asString(estPos[0],3),  12)  << " "
                   << rightJustify(asString(estPos[1],3),  12)  << " "
                   << rightJustify(asString(estPos[2],3),  12)  << " "
                   << rightJustify(asString(dt  ,3),       9)  << " "
                   << rightJustify(asString<int>(numFixed),4)  << " "
                   << rightJustify(asString(ratio,1),      4)  << " "
                   ;

           *outStr << rightJustify(asString(dLat,3), 6 )  << " "
                   << rightJustify(asString(dLon,3), 6 )  << " "
                   << rightJustify(asString(dH  ,3), 6 )  << " "
                   ;
           
           *outStr << endl;
       }
       else
       {
           double dx, dy, dz, dt;
           dx = getSolution(TypeID::dStaX,   state);
           dy = getSolution(TypeID::dStaY,   state);
           dz = getSolution(TypeID::dStaZ,   state);
           dt = getSolution(TypeID::dcdtSta, state);

           Triple xyzVec;
           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           Triple estPos = initialPos + xyzVec;

              // print time

           *outStr << rightJustify(asString(estPos[0],3), 12)<< " "
                   << rightJustify(asString(estPos[1],3), 12)<< " "
                   << rightJustify(asString(estPos[2],3), 12)<< " " 
                   << rightJustify(asString(dt,3),         9)<< " "
                   << rightJustify(asString<int>(numSat),   4  ) <<  " " 
                   ;

           *outStr << rightJustify(asString(dx,3),  6)<< " "
                   << rightJustify(asString(dy,3),  6)<< " "
                   << rightJustify(asString(dz,3),  6)<< " " 
                   ;

           dx = getSolution(TypeID::dStaX,   stateFixed);
           dy = getSolution(TypeID::dStaY,   stateFixed);
           dz = getSolution(TypeID::dStaZ,   stateFixed);
           dt = getSolution(TypeID::dcdtSta, stateFixed);

           xyzVec[0] = dx;
           xyzVec[1] = dy;
           xyzVec[2] = dz;

           if(debug)
           {
               cout << "xyzVec" << endl;
               cout << xyzVec << endl;
           }

           estPos = initialPos + xyzVec;

           if(debug)
               cout << "estPos:" << fixed  << setprecision(3) << setw(14) << estPos << endl;

           *outStr << rightJustify(asString(estPos[0],3),  12)<< " "
                   << rightJustify(asString(estPos[1],3),  12)<< " "
                   << rightJustify(asString(estPos[2],3),  12)<< " " 
                   << rightJustify(asString(dt,3),          9)<< " "
                   << rightJustify(asString<int>(numFixed), 3)<< " "
                   << rightJustify(asString(ratio,1),       4)  << " "
                   ;

           *outStr << rightJustify(asString(dx,3),  6)<< " "
                   << rightJustify(asString(dy,3),  6)<< " "
                   << rightJustify(asString(dz,3),  6)<< " " 
                   ;
           
           *outStr << endl;
       }
   }



   // print float solutions
   void SolverPPPAR::printAmbs( std::ofstream* outStr,
                              const CommonTime& epoch, 
                              VariableDataMap& BLCMap,
                              CovarianceDataMap& BLCCovMap)
   {
       for(auto amb: BLCMap)
       {
           SourceID source = amb.first.getSource();
           SatID sat       = amb.first.getSatellite();
           double satArc   = amb.first.getArc();
           TypeID type     = amb.first.getType();
           double value    = amb.second;
           double variance = BLCCovMap[amb.first][amb.first];
           double frac     = value - std::floor(value+0.5);

              // print time
           *outStr << YDSTime(epoch).printf("%Y %03j %14.6s") << " ";
           *outStr << source << " "
                   << sat << " "
                   << rightJustify(asString<int>(int(satArc)),6) << " "
                   << type << " "
                   << rightJustify(asString(value,6),13) << " "
                   << rightJustify(asString(variance,6),13) << " "
                   << rightJustify(asString(frac,6),13) << " "
                   << endl;
       }

   };

   // print mw values
   void SolverPPPAR::printFixedInfo( std::ofstream* outStr, 
                                     const CommonTime& epoch, 
                                     int numAmb,
                                     int numFixed)
   {
       *outStr << YDSTime(epoch).printf("%Y %03j % 12.6s") << " ";
       *outStr << rightJustify(asString<int>(numAmb),10) << " " 
               << rightJustify(asString<int>(numFixed),10) << " " 
               << endl;
   }

}  // End of namespace gpstk
